
#include <iostream>
#include <vector>
#include "filter.h"

using namespace std;
using namespace OMR;

void importSkeleton();

vector<Vector3>         jointsOri; // the original skeleton
vector<int>             prev;

int main() {
   
   importSkeleton();
   
   OMR::MotionFilter mfilter(jointsOri, prev);
   
   {
      vector<Transform<> > transfInput(jointsOri.size()-1);
      
      vector<Vector3> feet;
      feet.push_back(jointsOri[7]);
      feet.push_back(jointsOri[11]);
      feet.push_back(jointsOri[2]);
      mfilter.step(transfInput, feet);
      
      vector<Vector3> out = jointsOri;
      const vector<Transform<> > &t = mfilter.getTransforms();
      for(int i = 0; i < (int)out.size(); ++i) {
         out[i] = t[max(0, i - 1)] * out[i];
      }
      
      for (int i=0; i<out.size(); ++i) {
         cout<<i<<" : "<<out[i]<<endl;
      }
   }
   
   return 0;
}


void importSkeleton() {
   
   jointsOri.push_back(Vector3(0.488454, 0.510216, 0.466979));
   jointsOri.push_back(Vector3(0.487242, 0.421347, 0.472977));
   jointsOri.push_back(Vector3(0.486773, 0.371251, 0.473103));
   jointsOri.push_back(Vector3(0.488231, 0.72913, 0.467079));
   jointsOri.push_back(Vector3(0.436934, 0.371845, 0.473002));
   jointsOri.push_back(Vector3(0.425039, 0.257082, 0.474592));
   jointsOri.push_back(Vector3(0.42299, 0.142477, 0.489171));
   jointsOri.push_back(Vector3(0.413844, 0.131878, 0.576707));
   jointsOri.push_back(Vector3(0.538944, 0.372471, 0.473606));
   jointsOri.push_back(Vector3(0.558006, 0.25773, 0.478181));
   jointsOri.push_back(Vector3(0.561952, 0.139488, 0.488122));
   jointsOri.push_back(Vector3(0.56172, 0.135149, 0.57088));
   jointsOri.push_back(Vector3(0.416206, 0.499609, 0.466916));
   jointsOri.push_back(Vector3(0.323153, 0.389051, 0.461029));
   jointsOri.push_back(Vector3(0.238818, 0.323686, 0.497371));
   jointsOri.push_back(Vector3(0.562254, 0.501748, 0.460098));
   jointsOri.push_back(Vector3(0.66489, 0.381544, 0.449908));
   jointsOri.push_back(Vector3(0.716801, 0.280987, 0.501728));
   
   prev.push_back(-1);
   prev.push_back(0);
   prev.push_back(1);
   prev.push_back(0);
   prev.push_back(2);
   prev.push_back(4);
   prev.push_back(5);
   prev.push_back(6);
   prev.push_back(2);
   prev.push_back(8);
   prev.push_back(9);
   prev.push_back(10);
   prev.push_back(0);
   prev.push_back(12);
   prev.push_back(13);
   prev.push_back(0);
   prev.push_back(15);
   prev.push_back(16);
}
